#include "State_FreeStand.h"
#include "Dog.h"

#define KP_F 4.0f
#define KD_F 0.6f

#define KP_B 4.0f
#define KD_B 0.6f

void FreeStand_Enter(void)
{
    UART_Print("FreeStand enter\n");
}

void FreeStand_Run(void)
{
    // UART_Print("FreeStand run\n");

    HT_SetPosition(dog.legs[0].motorF, -STARTANGLE_F, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[0].motorB, -STARTANGLE_B, KP_B, KD_B, 0);
    HT_SetPosition(dog.legs[1].motorF, STARTANGLE_F, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[1].motorB, STARTANGLE_B, KP_B, KD_B, 0);
    HT_SetPosition(dog.legs[2].motorF, STARTANGLE_F, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[2].motorB, STARTANGLE_B, KP_B, KD_B, 0);
    HT_SetPosition(dog.legs[3].motorF, -STARTANGLE_F, KP_F, KD_F, 0);
    HT_SetPosition(dog.legs[3].motorB, -STARTANGLE_B, KP_B, KD_B, 0);
}
void FreeStand_Exit(void)
{
    UART_Print("FreeStand exit\n");
}